#include "ERR.h"

extern uint8_t flightMode;//飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
extern uint8_t modePre;//飞行模式预装载，如果没有问题才会覆盖飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
extern unsigned long tStamp[3];//用于收集各传感器更新事件戳，以此判断是否失效,0遥控，1攻角，2陀螺仪
extern float input[6];//输入数据，横滚0、俯仰1、油门2、偏航3、直接力4、舵面来流指向5，归一法

uint8_t emgcMode = 0;//紧急模式，0bit失控，1bit攻角失联，2bit角速度失联

void ERR()
{
  ERR_HAD();//检测错误
  ERR_MSU();//错误措施
}

void ERR_HAD()
{
  if (millis() - tStamp[0] >= 1500)
  {
    emgcMode |= (1 << 0);//上报遥控器失效
  }
  else
  {
    emgcMode &= ~(1 << 0);//取消遥控器失效
  }

  if (millis() - tStamp[1] >= 400)
  {
    emgcMode |= (1 << 1);//上报攻角传感器失效
  }
  else
  {
    emgcMode &= ~(1 << 1);//取消攻角传感器失效
  }

  if (millis() - tStamp[2] >= 400)
  {
    emgcMode |= (1 << 2);//上报陀螺仪失效
  }
  else
  {
    emgcMode &= ~(1 << 2);//取消陀螺仪失效
  }
}

void ERR_MSU()
{
  flightMode = modePre;//先正常读取参数
  switch (emgcMode)
  {
    case 0b001://遥控器失效模式
      //飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
      /*flightMode = 0;//改为自稳模式
      input[0] = 0;//横滚归0
      input[1] = -0.3;//向下20°俯冲
      input[2] = -1;//电机为0
      input[3] = 0;//偏航为0
      input[4] = 0;//直接力控制为0
      input[5] = 1;//AOA控制正常工作*/
      flightMode = 3;//改为双闭环模式
      input[0] = 0;//横滚归0
      input[1] = 0;//向下20°俯冲
      input[2] = -1;//电机为0
      input[3] = 0;//偏航为0
      input[4] = 0;//直接力控制为0
      input[5] = 1;//来流指向正常工作*/
      break;

    case 0b010://攻角传感器失效模式
      //飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
      //在攻角传感器失效的情况下认为静不稳定鸭翼不太具备救机的可能性，但还是挣扎一下以半自稳飞回来
      if ((modePre == 2) || (modePre == 3))
      {//如果在使用攻角模式和双闭环模式
        flightMode = 4;//切入半自稳模式
      }
      input[5] = 0;//关闭来流指向
      break;

    case 0b100://陀螺仪失效模式
      //陀螺仪失效情况下，认为还可以正常飞行
      if ((modePre == 0) || (modePre == 3) || (modePre == 4))
      {//如果是自稳、双闭环、半自稳状态
        flightMode = 2;//切入攻角模式
      }
      break;

    case 0b011://陀螺仪、攻角传感器失效模式
      //一般认为是不太可能救机的，但万一呢
      flightMode = 1;//切入手动控制模式
      input[5] = 0;//关闭来流指向
      break;

    case 0b111://严重失控模式，手动控制，所有运算为0
      //没救了，等死吧
      flightMode = 1;//手动模式
      input[0] = 0;//横滚回中
      input[1] = 0;//俯仰回中
      input[2] = -1;//油门最低
      input[3] = 0;//偏航回中
      input[4] = 0;//直接力控制回中
      input[5] = 0;//关闭舵面来流指向
      break;
  }
}
